#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  encoder1,       sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  encoder2,       sensorQuadEncoder)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           frontleft,     tmotorVex269_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           backleft,      tmotorVex269_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port9,           backright,     tmotorVex269_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port10,          frontright,    tmotorVex269_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void driveStraight(float feet)
{
	float distance=200*feet;//we measured and found that 200 is a good value
	nMotorEncoder[frontright]  =0;
	nMotorEncoder[frontleft]  =0;
	float distanceTraveled = 0;
	int rightSensor;
	int leftSensor;
	int rightPower;
	int leftPower;

	while (distance >= distanceTraveled)
	{
		rightSensor = nMotorEncoder[frontright];
		leftSensor = -nMotorEncoder[frontleft];
		distanceTraveled = rightSensor;
		if(leftSensor>rightSensor){
			rightPower =127;
			leftPower =127-(leftSensor-rightSensor);
		}
		else if(rightSensor>leftSensor){
			leftPower =127;
			rightPower =127-(rightSensor-leftSensor);

		}
		else
		{
			rightPower =127;
			leftPower =127;
		}



		motor[frontright] = rightPower;
		motor[frontleft] = leftPower;
		motor[backleft] = leftPower;
		motor[backright] = rightPower;

	}
	motor[frontright] = 0;
	motor[frontleft] = 0;
	motor[backright] = 0;
	motor[backleft] = 0;
}



void  turn(float degrees)
{
	float distance=200*degrees;//we measured and found that 200 is a good value
	nMotorEncoder[frontright]  =0;
	nMotorEncoder[frontleft]  =0;
	float distanceTraveled = 0;
	int rightSensor;
	int leftSensor;
	int rightPower;
	int leftPower;

	while (distance >= distanceTraveled)
	{
		if(degrees>0){
			//turn right
		rightSensor = nMotorEncoder[frontright];
			leftSensor = nMotorEncoder[frontleft];
			distanceTraveled = rightSensor;
			if(leftSensor>rightSensor){
				rightPower =-127;
				leftPower =127-(leftSensor-rightSensor);
			}
			else if(rightSensor>leftSensor){
				leftPower =127;
				rightPower =-(127-(rightSensor-leftSensor));

			}
			else {
				rightPower =-127;
				leftPower =127;
			}
		}
		else {
			//turning left
			rightSensor = nMotorEncoder[frontright];
			leftSensor = nMotorEncoder[frontleft];
			distanceTraveled = rightSensor;
			if(leftSensor>rightSensor){
				rightPower =127;
				leftPower =-(127-(leftSensor-rightSensor));
			}
			else if(rightSensor>leftSensor){
				leftPower =-127;
				rightPower =(127-(rightSensor-leftSensor));

			}
			else {
				rightPower =127;
				leftPower =-127;
			}





		}

		motor[frontright] = rightPower;
		motor[frontleft] = leftPower;
		motor[backleft] = leftPower;
		motor[backright] = rightPower;

	}
	motor[frontright] = 0;
	motor[frontleft] = 0;
	motor[backright] = 0;
	motor[backleft] = 0;
}


void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;
	nMotorEncoder[frontright]  =0;
	nMotorEncoder[frontleft]  =0;
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	driveStraight(4);
//	turn(90);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// User control code here, inside the loop
	driveStraight(4);
//	turn(-90);
}
